import java.awt.event.ActionEvent;
import java.awt.event.ActionListener;

import javax.swing.*;
import java.awt.*;

// added by students, to keep track of which sensors we want
import java.util.ArrayList;


/**
 * @author Joseph
 * @date 10/23/2008
 */
public class ControlPanel extends JPanel implements ActionListener {

    protected JButton Up,Down,Left,Right,Stop, Open,Mode,CliffRight;
    protected JPanel p1,p2,p3;
    protected JTextArea message;
    OpenComPort iRobotBAM = new OpenComPort();
    //OpenComPort iRobotBAM;

    // added by students
    protected JButton GoDistance, TurnAngle, MakeSquare, GoVelocity, ReadSensors, FollowWall, Bug2, TurnRadius;
    protected JPanel pProf, pStudent, p4, p5, p6, p7, SensorButtons;
    protected GPS Display; //= new GPS("");
    protected ArrayList<String> SensorsOn = new ArrayList<String>();
    protected StudentActions sActions = new StudentActions(iRobotBAM, Display);
    protected Boolean debugON = true;

    // simple function to print debugging info, does nothing in debugON = false
    protected void debugPrint(Object o){
	if (debugON) {System.out.print("DEBUG: "); System.out.println(o);}
    }

	
    public ControlPanel() { //debug 
    						//Display.addPoint(70,10);
    	Open = new JButton("Open");
       	Open.setActionCommand("Open");

    	Mode = new JButton("Mode");
    	Mode.setActionCommand("Mode");
    	Mode.setEnabled(false);
    	
    	Up = new JButton("Go");
    	Up.setActionCommand("Go");
    	Up.setEnabled(false);
    	
    	Down = new JButton("Back");
    	Down.setActionCommand("Back");
    	Down.setEnabled(false);
    	
    	Left = new JButton("Turn Left");
    	Left.setActionCommand("Turn Left");
    	Left.setEnabled(false);
    	
    	Right = new JButton("Turn Right");
    	Right.setActionCommand("Turn Right");
    	Right.setEnabled(false);
    	
    	Stop = new JButton("Stop");
    	Stop.setActionCommand("Stop");
    	Stop.setEnabled(false);
    	
    	//CliffRight = new JButton("CliffRight");
    	//CliffRight.setActionCommand("CliffRight");
    	//CliffRight.setEnabled(false);
    	
	//************************************************
	// added by students
	GoDistance = new JButton("Go Distance");
	GoDistance.setActionCommand("Go Distance");
    GoDistance.setEnabled(false);

	TurnAngle = new JButton("Turn Angle");
	TurnAngle.setActionCommand("Turn Angle");
	TurnAngle.setEnabled(false);
	
	MakeSquare = new JButton("Make Square");
	MakeSquare.setActionCommand("Make Square");
	MakeSquare.setEnabled(false);

	GoVelocity = new JButton("Go Velocity");
	GoVelocity.setActionCommand("Go Velocity");
	GoVelocity.setEnabled(false);

	ReadSensors = new JButton("Read Sensors");
	ReadSensors.setActionCommand("Read Sensors");
	ReadSensors.setEnabled(false);
	
	FollowWall = new JButton("Follow Wall");
	FollowWall.setActionCommand("Follow Wall");
	FollowWall.setEnabled(false);

	Bug2 = new JButton("BUG 2");
	Bug2.setActionCommand("BUG 2");
	Bug2.setEnabled(false);
	
	TurnRadius = new JButton("Turn Radius");
	TurnRadius.setActionCommand("Turn Radius");
	TurnRadius.setEnabled(false);
	
	//Sensors = new JCheckBox();
	//************************************************


        //Listen for actions on buttons 1 and 3.
    	Open.addActionListener(this);
    	Mode.addActionListener(this);
    	Up.addActionListener(this);
    	Down.addActionListener(this);
    	Left.addActionListener(this);
    	Right.addActionListener(this);
    	Stop.addActionListener(this);
    	//CliffRight.addActionListener(this);
	
	// added by students
	GoDistance.addActionListener(this);
	TurnAngle.addActionListener(this);
	MakeSquare.addActionListener(this);
	GoVelocity.addActionListener(this);
	ReadSensors.addActionListener(this);
	FollowWall.addActionListener(this);
	Bug2.addActionListener(this);
	TurnRadius.addActionListener(this);

       	p1=new JPanel(new BorderLayout());
    	p2=new JPanel(new BorderLayout());
    	p3=new JPanel(new BorderLayout());
        // added by students
	pProf=new JPanel(new BorderLayout());
	pStudent=new JPanel(new BorderLayout());
	p4=new JPanel(new BorderLayout());
	p5=new JPanel(new BorderLayout());
	p6=new JPanel(new BorderLayout());
	p7=new JPanel(new BorderLayout());
	SensorButtons = createSensorsCheckPane();

    	//Add Components to this container, using the default FlowLayout.
        p1.add(Open,BorderLayout.WEST);
        p1.add(Mode,BorderLayout.EAST);
        p2.add(Up,BorderLayout.NORTH);
        p2.add(Down,BorderLayout.SOUTH);
        p2.add(Left,BorderLayout.WEST);
        p2.add(Right,BorderLayout.EAST);
        p2.add(Stop,BorderLayout.CENTER);
        //p3.add(CliffRight,BorderLayout.CENTER);
        //message =new JTextArea(1,3);
        //p3.add(message,BorderLayout.EAST);
        // added by students
	p4.add(GoDistance,BorderLayout.CENTER);
	p5.add(TurnAngle,BorderLayout.WEST);
	p5.add(MakeSquare,BorderLayout.CENTER);
	p5.add(GoVelocity,BorderLayout.EAST);
	p6.add(ReadSensors,BorderLayout.NORTH);
	p6.add(FollowWall,BorderLayout.CENTER);
	p7.add(Bug2,BorderLayout.NORTH);
	p7.add(TurnRadius,BorderLayout.CENTER);
	p6.add(p7,BorderLayout.SOUTH);
        setLayout(new BorderLayout());
        //add(p1,BorderLayout.NORTH);
        //add(p2,BorderLayout.EAST);
        //add(p3,BorderLayout.SOUTH);
	// added by student
	pProf.add(p1,BorderLayout.NORTH);
	pProf.add(p2,BorderLayout.CENTER);
	add(pProf,BorderLayout.NORTH);
	pStudent.add(p4,BorderLayout.NORTH);
	pStudent.add(p5,BorderLayout.CENTER);
	pStudent.add(p6,BorderLayout.SOUTH);
	add(pStudent,BorderLayout.CENTER);
	add(SensorButtons,BorderLayout.SOUTH);
	
    }

    /**
     * Create the GUI and show it.  For thread safety, 
     * this method should be invoked from the 
     * event-dispatching thread.
     */
    public void createAndShowGUI() {

        //Create and set up the window.
        JFrame frame = new JFrame("ControlPanel");
        frame.setDefaultCloseOperation(JFrame.EXIT_ON_CLOSE);

        //Create and set up the content pane.
        ControlPanel newContentPane = new ControlPanel();
        newContentPane.setOpaque(true); //content panes must be opaque
        frame.setContentPane(newContentPane);


        //Display the window.
        frame.pack();
        frame.setVisible(true);
    }
	
	/* (non-Javadoc)
	 * @see java.awt.event.ActionListener#actionPerformed(java.awt.event.ActionEvent)
	 */
	@Override
	public void actionPerformed(ActionEvent e) {
		// TODO Auto-generated method stub
		if ("Open".equals(e.getActionCommand())) {
			Mode.setEnabled(true);
			if(iRobotBAM.SetupBAM()==0){
				Open.setEnabled(false);
			}
		}
		if ("Mode".equals(e.getActionCommand())) {
			
			SetMode();
			LED_On();
						
			Up.setEnabled(true);
			Down.setEnabled(true);
			Right.setEnabled(true);
			Left.setEnabled(true);
			Stop.setEnabled(false);
			//CliffRight.setEnabled(true);
		    GoDistance.setEnabled(true);
			TurnAngle.setEnabled(true);
			MakeSquare.setEnabled(true);
			GoVelocity.setEnabled(true);
			ReadSensors.setEnabled(true);
			FollowWall.setEnabled(true);
			Bug2.setEnabled(true);
			TurnRadius.setEnabled(true);
		}
		
		if ("Go".equals(e.getActionCommand())) {
			sActions.GoVelocity(200);
			
			Up.setEnabled(false);
			Down.setEnabled(false);
			Right.setEnabled(false);
			Left.setEnabled(false);
			Stop.setEnabled(true);
		    GoDistance.setEnabled(false);
			TurnAngle.setEnabled(false);
			MakeSquare.setEnabled(false);
			GoVelocity.setEnabled(false);
			ReadSensors.setEnabled(false);
			FollowWall.setEnabled(false);
			Bug2.setEnabled(false);
			TurnRadius.setEnabled(false);
		}
		
		if ("Back".equals(e.getActionCommand())) {
			sActions.GoVelocity(-200);
			
			Up.setEnabled(false);
			Down.setEnabled(false);
			Right.setEnabled(false);
			Left.setEnabled(false);
			Stop.setEnabled(true);
		    GoDistance.setEnabled(false);
			TurnAngle.setEnabled(false);
			MakeSquare.setEnabled(false);
			GoVelocity.setEnabled(false);
			ReadSensors.setEnabled(false);
			FollowWall.setEnabled(false);
			Bug2.setEnabled(false);
			TurnRadius.setEnabled(false);
		}
		
		if ("Turn Right".equals(e.getActionCommand())) {
			sActions.TurnVelocity(-200);
			
			Up.setEnabled(false);
			Down.setEnabled(false);
			Right.setEnabled(false);
			Left.setEnabled(false);
			Stop.setEnabled(true);
		    GoDistance.setEnabled(false);
			TurnAngle.setEnabled(false);
			MakeSquare.setEnabled(false);
			GoVelocity.setEnabled(false);
			ReadSensors.setEnabled(false);
			FollowWall.setEnabled(false);
			Bug2.setEnabled(false);
			TurnRadius.setEnabled(false);
		}
		
		if ("Turn Left".equals(e.getActionCommand())) {
			sActions.TurnVelocity(200);
			
			Up.setEnabled(false);
			Down.setEnabled(false);
			Right.setEnabled(false);
			Left.setEnabled(false);
			Stop.setEnabled(true);
		    GoDistance.setEnabled(false);
			TurnAngle.setEnabled(false);
			MakeSquare.setEnabled(false);
			GoVelocity.setEnabled(false);
			ReadSensors.setEnabled(false);
			FollowWall.setEnabled(false);
			Bug2.setEnabled(false);
			TurnRadius.setEnabled(false);
		}
		
		if ("Stop".equals(e.getActionCommand())) {
			sActions.StopPublic();
			
            Up.setEnabled(true);
			Down.setEnabled(true);
			Right.setEnabled(true);
			Left.setEnabled(true);
			Stop.setEnabled(true);
			//CliffRight.setEnabled(true);
		    GoDistance.setEnabled(true);
			TurnAngle.setEnabled(true);
			MakeSquare.setEnabled(true);
			GoVelocity.setEnabled(true);
			ReadSensors.setEnabled(true);
			FollowWall.setEnabled(true);
			Bug2.setEnabled(true);
			TurnRadius.setEnabled(true);
		}
		/*
		if("CliffRight".equals(e.getActionCommand())){
			byte[] data = new byte[2];
			data[0]= (byte)142;
			data[1]=(byte)11;
			//iRobotBAM.Write(data);
			byte [] sensor =new byte[1];
			//iRobotBAM.Read(sensor);
			int x= sensor[0];
			Integer y= (Integer)x;
			System.out.println(sensor[0]);
			message.insert(y.toString(),0);	
			
		}
		*/

		// added by students
		/**
		 * what happens when we press "Go Distance"
		 */
		if("Go Distance".equals(e.getActionCommand())){
		    debugPrint("Go Distance button pressed");
		    String s = (String)JOptionPane.showInputDialog("Enter distance (0 to cancel)");
		    debugPrint("desired distance: " + s);
		    if (s == null)
				{debugPrint("entered NULL, do nothing");}
		    else {
		    	int dist;
		    	try {
		    		dist = Integer.parseInt(s);}
		    	catch (NumberFormatException e_nfe) {
		    		debugPrint("entry not a number, do nothing"); 
		    		dist = 0;
		    	}
		    	debugPrint("call launch GoDistance("+dist+")");
		    	sActions.GoDistance(dist);
		    }
		}

		/**
		 * what happens when we press "Turn Angle"
		 */
		if("Turn Angle".equals(e.getActionCommand())){
		    debugPrint("Turn Angle button pressed");
		    String s = (String)JOptionPane.showInputDialog("Enter angle (0 to cancel)");
		    debugPrint("desired angle: " + s);
		    if (s == null)
			{debugPrint("entered NULL, do nothing");}
		    else {
			int angle;
			try {
			    angle = Integer.parseInt(s);}
			catch (NumberFormatException e_nfe) {
			    debugPrint("entry not a number, do nothing"); 
			    angle = 0;
			}
			if (angle == 0) {
			    debugPrint("entry = 0, do nothing");
			}
			else { // perform action
			    debugPrint("call launch TurnAngle("+angle+")");
			    sActions.TurnAngle(angle);
			}
		    }
		}

		/**
		 * what happens when we press "Make Square"
		 */
		if("Make Square".equals(e.getActionCommand())){
		    debugPrint("Make Square button pressed");
		    sActions.MakeSquare(); 
		}

		/**
		 * what happens when we press "Go Velocity"
		 */
		if("Go Velocity".equals(e.getActionCommand())){
		    debugPrint("Go Velocity button pressed");
		    String s = (String)JOptionPane.showInputDialog("Enter velocity (0 to cancel)");
		    debugPrint("desired velocity: " + s);
		    if (s == null)
			{debugPrint("entered NULL, do nothing");}
		    else {
			int velo;
			try {
			    velo = Integer.parseInt(s);}
			catch (NumberFormatException e_nfe) {
			    debugPrint("entry not a number, do nothing"); 
			    velo = 0;
			}
			if (velo <= 0) {
			    debugPrint("entry <= 0, do nothing");
			}
			else if (velo > 500) {
			    debugPrint("entry velo > 500, do nothing");
			}
			else { // perform action
			    debugPrint("call launch GoVelocity("+velo+")");
			    sActions.GoVelocity(velo);
			}
		    }
		}

		/**
		 * what happens when we press "Read Sensors"
		 */
		if("Read Sensors".equals(e.getActionCommand())){
		    debugPrint("Read Sensors button pressed");
		    String sensors_message = sActions.ReadSensors(SensorsOn); 
		    debugPrint("message from sensors: " + sensors_message);
		    writeMessage(sensors_message);
		}

		if ("Follow Wall".equals(e.getActionCommand())){
			debugPrint("Follow Wall button pressed");
			String dialog = "Enter how many iterations: (ex. 15)";
			String s = (String)JOptionPane.showInputDialog(dialog);
			//String s = "1";
			debugPrint("desired iterations: " + s);
			if (s == null){
				debugPrint("entered NULL, do nothing");
			}
		    else {
		    	int iterations;
		    	try {
		    		iterations = Integer.parseInt(s);
		    	}
		    	catch (NumberFormatException e_nfe) {
		    		debugPrint("entry not a number, do nothing"); 
		    		iterations = 0;
		    	}
			    debugPrint("call launch FollowWall("+iterations+")");
			    sActions.FollowWall(iterations);
			}			
		}
		
		if ("BUG 2".equals(e.getActionCommand())) {
			double goal[] = new double[2];
			debugPrint("BUG 2 button pressed");
			String dialog;
			dialog = "Enter where you would like to go.\n";
			dialog += "FYI, you are currently at ";
			dialog += "("+(int)sActions.getPositions()[0]+", "+(int)sActions.getPositions()[1]+")\n";
			dialog += "Your orientation is "+(int)sActions.getPositions()[2]+"degrees.\n";
			dialog += "Remember, units are in millimeters!!!\n\n";
			dialog += "Enter xpos:";
			String s = (String)JOptionPane.showInputDialog(dialog);
			if (s == null){
				debugPrint("entered NULL, do nothing");
			}
		    else {
		    	goal[0] = Integer.parseInt(s);
		    	dialog = "Enter ypos:";
		    	s = (String)JOptionPane.showInputDialog(dialog);
		    	if (s==null){
		    		debugPrint("entered NULL, do nothing");
		    	}
		    	else{
		    		goal[1] = Integer.parseInt(s);
		    	   	sActions.Bug2( goal );
		    	}
		    }
		}
		
		if ("Turn Radius".equals(e.getActionCommand())) {
		    String s = (String)JOptionPane.showInputDialog("Enter distance");
		    int dist = Integer.parseInt(s);
		    s = (String)JOptionPane.showInputDialog("Enter radius");
		    int radius = Integer.parseInt(s);
		    sActions.Turn(dist,radius);
		}
	}
	
	private void SetMode()
    {
		byte[] data = new byte[2];
		
		data[0] = (byte)128;  //Direct Drive command
        data[1] = (byte)132;  //Safe Mode
		
        iRobotBAM.Write(data);
    }

    private void LED_On()
    {
        byte[] data = new byte[4];

        data[0] = (byte)139;  //LED command
        data[1] = (byte)8;    //Select LED (Play : 8, power : 2) 
        data[2] = (byte)0;    //Color 0 = green, 255 = red
        data[3] = (byte)128;  //Intensity

        iRobotBAM.Write(data);

        data[0] = (byte)139;  //LED command
        data[1] = (byte)2;    //Select LED (Play : 8, power : 2) 
        data[2] = (byte)255;    //Color 0 = green, 255 = red
        data[3] = (byte)128;  //Intensity

        iRobotBAM.Write(data);
    }

    

    // added by students
   
    /** 
     * create a list of checkboxes (different sensors)
     * when one of the button is pressed, the name of the sensors is added
     * or removed from the list SensorsOn
     */
    protected JPanel createSensorsCheckPane()
    {
	class JPanelListen extends JPanel implements ActionListener{
	    public void actionPerformed(ActionEvent e){
		String sensor = e.getActionCommand();
		if (((AbstractButton)e.getSource()).isSelected())
		    {SensorsOn.add(sensor); debugPrint("adding sensor: " + sensor);}
		else
		    {SensorsOn.remove(sensor); debugPrint("removing sensor: " + sensor);}
		debugPrint(SensorsOn);
	    };
	}

	JPanelListen panelLeft = new JPanelListen();
	JPanelListen panelRight = new JPanelListen();
	panelLeft.setLayout(new BoxLayout(panelLeft,BoxLayout.Y_AXIS));
	panelRight.setLayout(new BoxLayout(panelRight,BoxLayout.Y_AXIS));

	// buttons
	JCheckBox c1 = new JCheckBox("wheeldropCaster");
	c1.addActionListener(panelLeft);
	panelLeft.add(c1);
	JCheckBox c2 = new JCheckBox("wheeldropLeft");
	c2.addActionListener(panelRight);
	panelRight.add(c2);
	JCheckBox c3 = new JCheckBox("wheeldropRight");
	c3.addActionListener(panelLeft);
	panelLeft.add(c3);
	JCheckBox c4 = new JCheckBox("bumpLeft");
	c4.addActionListener(panelRight);
	panelRight.add(c4);
	JCheckBox c5 = new JCheckBox("bumpRight");
	c5.addActionListener(panelLeft);
	panelLeft.add(c5);
	JCheckBox c6 = new JCheckBox("wall");
	c6.addActionListener(panelRight);
	panelRight.add(c6);
	JCheckBox c7 = new JCheckBox("cliffLeft");
	c7.addActionListener(panelLeft);
	panelLeft.add(c7);
	JCheckBox c8 = new JCheckBox("cliffFrontLeft");
	c8.addActionListener(panelRight);
	panelRight.add(c8);
	JCheckBox c9 = new JCheckBox("cliffFrontRight");
	c9.addActionListener(panelLeft);
	panelLeft.add(c9);
	JCheckBox c10 = new JCheckBox("cliffRight");
	c10.addActionListener(panelRight);
	panelRight.add(c10);
	JCheckBox c11 = new JCheckBox("virtualWall");
	c11.addActionListener(panelLeft);
	panelLeft.add(c11);	
	
	// create panel, add left and right subpanels
	JPanel panel = new JPanel(new BorderLayout());
	panel.add(panelLeft,BorderLayout.WEST);
	panel.add(panelRight,BorderLayout.EAST);

	// finally add message inbox
	message =new JTextArea(1,3);
	panel.add(message,BorderLayout.SOUTH);
	//panelRight.add(message);

	return panel;
    }


    /**
     * write something to the 'message borad', line at the bottom of the GUI
     */
    protected void writeMessage(String s)
    {
	message.setLineWrap(true);
	message.setText(s);
    }

}
